﻿using System;
// define float as Scalar
using Scalar = System.Single;

namespace OhioState.Math
{
    class BoundCondition
    {
        public static Scalar SCALAR_EPSILON = 0.001f;
    }

    public interface Matrix : IMatrix
    {
    }

    #region Matrix2

    //
    // Matrix2
    //
    [Serializable]
    public class Matrix2 : Matrix
    {
        // data member
        private Vector2[] _matrix2 = new Vector2[2];

        #region Constructors
        public Matrix2()
        {
            _matrix2[0] = new Vector2();
            _matrix2[1] = new Vector2();
            Identity();
        }

        public Matrix2(Vector2[] v)
        {
            _matrix2[0] = new Vector2(v[0]);
            _matrix2[1] = new Vector2(v[1]);

        }

        public Matrix2(Matrix2 m)
        {
            _matrix2[0] = new Vector2(m._Matrix2[0]);
            _matrix2[1] = new Vector2(m._Matrix2[1]);
        }

        public Matrix2(Vector2 a, Vector2 b)
        {
            _matrix2[0] = new Vector2(a);
            _matrix2[1] = new Vector2(b);
        }

        public Matrix2(Scalar m0, Scalar m1, Scalar m2, Scalar m3)
        {
            _matrix2[0] = new Vector2();
            _matrix2[1] = new Vector2();

            _matrix2[0].InternalVector[0] = m0;
            _matrix2[0].InternalVector[1] = m1;

            _matrix2[1].InternalVector[0] = m2;
            _matrix2[1].InternalVector[1] = m3;
        }
        #endregion

        // property
        public Vector2[] _Matrix2
        {
            get { return this._matrix2; }
            set { this._matrix2 = value; }
        }

        public Scalar this[int row, int column]
        {
            get
            {
                return _matrix2[row].InternalVector[column];
            }
            set
            {
                //
                // We could also do asserts, which would avoid performance penalties on 
                //    the release version. We really need this to be in-lined for the
                //    operators below.
                //
                if ((row >= 0) && (row < 2) && (column >= 0) && (column < 2))
                    _matrix2[row].InternalVector[column] = value;
                else
                    throw (new IndexOutOfRangeException());
            }
        }

        // public methods
        public void GetValue(out Scalar[] t)
        {
            t = new Scalar[16];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    t[3*i + j] = _matrix2[i].InternalVector[j];
                }
            }
  
        }

        public Vector2 getRowVector(int row)
        {
            return new Vector2(_matrix2[row]);
        }

        public void setRowVector(int row, Vector2 v)
        {
            _matrix2[row] = new Vector2(v);
        }

        public Vector2 getColVector(int column)
        {
            Vector2 rv = new Vector2();
            rv.InternalVector[0] = _matrix2[0].InternalVector[column];
            rv.InternalVector[1] = _matrix2[1].InternalVector[column];
            return rv;
        }

        public void setColVector(int column, Vector2 v)
        {
            _matrix2[0].InternalVector[column] = v.InternalVector[0];
            _matrix2[1].InternalVector[column] = v.InternalVector[1];
        }

        public Scalar[] GetStreamRowOrder()
        {
            Scalar[] result = new Scalar[4];
            int l = 0;
            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    result[l++] = _matrix2[i].InternalVector[j];

            return result;
        }

        public Scalar[] GetStreamColumnOrder()
        {
            Scalar[] result = new Scalar[4];
            int l = 0;
            for (int j = 0; j < 2; j++)
                for (int i = 0; i < 2; i++)
                    result[l++] = _matrix2[i].InternalVector[j];
            return result;
        }

        public void Zero()
        {
            _matrix2[0].Zero();
            _matrix2[1].Zero();
        }

        public void Negate()
        {
            _matrix2[0].negate();
            _matrix2[1].negate();
        }

        public void Print()
        {
            _matrix2[0].print();
            _matrix2[1].print();
        }

        public void Identity()
        {
            _matrix2[0].Zero();
            _matrix2[1].Zero();
            _matrix2[0].InternalVector[0] = 1.0F;
            _matrix2[1].InternalVector[1] = 1.0F;
        }

        public Matrix2 transpose(Matrix2 m)
        {
            Matrix2 rm = new Matrix2();
            rm.setRowVector(0, m.getColVector(0));
            rm.setRowVector(1, m.getColVector(1));
            return rm;
        }

     	//
        // inverse function 
        //
        public static Matrix2 inverse(Matrix2 m)
    	{
            Scalar det = 1.0f / (m._Matrix2[0].InternalVector[0] * m._Matrix2[1].InternalVector[1] - m._Matrix2[0].InternalVector[1] * m._Matrix2[1].InternalVector[0]);
            Matrix2 result = new Matrix2(
                det * m._Matrix2[1].InternalVector[1],
                -det * m._Matrix2[0].InternalVector[1],
                -det * m._Matrix2[1].InternalVector[0],
                det * m._Matrix2[0].InternalVector[0]);

	        return result;
    	}

	    //
        // safe inverse, verfying the resulting inverse matrix
        //
        public static bool safe_inverse(Matrix2 m)
	    {
            Matrix2 inv = new Matrix2();
            inv = Matrix2.inverse(m);

	        Matrix2 l = inv * m;
            Matrix2 r = m * inv;

            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    if (System.Math.Abs(l._Matrix2[i].InternalVector[j] - r._Matrix2[i].InternalVector[j]) > BoundCondition.SCALAR_EPSILON)
                        return false;
            return true;                      
        }

        // operator functions
        public static Matrix2 operator +(Matrix2 a, Matrix2 b)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0] = a._Matrix2[0] + b._Matrix2[0];
            temp._Matrix2[1] = a._Matrix2[1] + b._Matrix2[1];
            return temp;
        }

        public static Matrix2 operator -(Matrix2 m)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0].negate();
            temp._Matrix2[1].negate();
            return temp;
        }

        public static Matrix2 operator -(Matrix2 a, Matrix2 b)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0] = a._Matrix2[0] - b._Matrix2[0];
            temp._Matrix2[1] = a._Matrix2[1] - b._Matrix2[1];
            return temp;
        }

        public static Matrix2 operator *(Matrix2 a, Matrix2 b)
        {
            Matrix2 temp = new Matrix2();
            for (int i = 0; i < 2; i++)
                for (int j = 0; j < 2; j++)
                    temp._Matrix2[i].InternalVector[j] = a._Matrix2[i].dot(b.getColVector(j));
            return temp;
        }

        public static Matrix2 operator *(Scalar s, Matrix2 b)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0] = s * b._Matrix2[0];
            temp._Matrix2[1] = s * b._Matrix2[1];
            return temp;
        }

        public static Matrix2 operator *(Matrix2 a, Scalar s)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0] = a._Matrix2[0] * s;
            temp._Matrix2[1] = a._Matrix2[1] * s;
            return temp;
        }

        public static Vector2 operator *(Matrix2 a, Vector2 v)
        {
            Vector2 temp = new Vector2();
            temp.InternalVector[0] = a._Matrix2[0].dot(v);
            temp.InternalVector[1] = a._Matrix2[1].dot(v);
            return temp;
        }

        public static Matrix2 operator /(Matrix2 a, Scalar s)
        {
            Matrix2 temp = new Matrix2();
            temp._Matrix2[0] = a._Matrix2[0] / s;
            temp._Matrix2[1] = a._Matrix2[1] / s;
            return temp;
        }

        public static bool operator ==(Matrix2 a, Matrix2 b)
        {
            if (a._Matrix2[0] != b._Matrix2[0]) return false;
            if (a._Matrix2[1] != b._Matrix2[1]) return false;
            return true;
        }

        public static bool operator !=(Matrix2 a, Matrix2 b)
        {
            return !(a == b);
        }

        public override bool Equals(Object obj)
        {
            return obj is Matrix2 && this == (Matrix2)obj;
        }

        public override int GetHashCode()
        {
            return _matrix2.GetHashCode();
        }

    }

    #endregion

    #region Matrix3
    //
    // Matrix3
    //
    [Serializable]
    public class Matrix3 : Matrix
    {
        // data member
        private Vector3[] _matrix3 = new Vector3[3];

        #region Constructors
        public Matrix3()
        {
            _matrix3[0] = new Vector3();
            _matrix3[1] = new Vector3();
            _matrix3[2] = new Vector3();
            Identity();
        }

        public Matrix3(Vector3[] v)
        {
            _matrix3[0] = new Vector3(v[0]);
            _matrix3[1] = new Vector3(v[1]);
            _matrix3[2] = new Vector3(v[2]);

        }

        public Matrix3(Matrix3 m)
        {
            _matrix3[0] = new Vector3(m._Matrix3[0]);
            _matrix3[1] = new Vector3(m._Matrix3[1]);
            _matrix3[2] = new Vector3(m._Matrix3[2]);
        }

        public Matrix3(Vector3 a, Vector3 b, Vector3 c)
        {
            _matrix3[0] = new Vector3(a);
            _matrix3[1] = new Vector3(b);
            _matrix3[2] = new Vector3(c);
        }

        public Matrix3(Scalar m0, Scalar m1, Scalar m2,
                       Scalar m3, Scalar m4, Scalar m5,
                       Scalar m6, Scalar m7, Scalar m8)
        {
            _matrix3[0] = new Vector3();
            _matrix3[1] = new Vector3();
            _matrix3[2] = new Vector3();

            _matrix3[0]._Vector3[0] = m0;
            _matrix3[0]._Vector3[1] = m1;
            _matrix3[0]._Vector3[2] = m2;

            _matrix3[1]._Vector3[0] = m3;
            _matrix3[1]._Vector3[1] = m4;
            _matrix3[1]._Vector3[2] = m5;

            _matrix3[2]._Vector3[0] = m6;
            _matrix3[2]._Vector3[1] = m7;
            _matrix3[2]._Vector3[2] = m8;
        }
        #endregion

        // property
        public Vector3[] _Matrix3
        {
            get { return this._matrix3; }
            set { this._matrix3 = value; }
        }

        public Scalar this[int row, int column]
        {
            get
            {
                return _matrix3[row]._Vector3[column];
            }
            set
            {
                //
                // We could also do asserts, which would avoid performance penalties on 
                //    the release version. We really need this to be in-lined for the
                //    operators below.
                //
                if ((row >= 0) && (row < 3) && (column >= 0) && (column < 3))
                    _matrix3[row]._Vector3[column] = value;
                else
                    throw (new IndexOutOfRangeException());
            }
        }

        // public methods
        public void GetValue(out Scalar[] t)
        {
            t = new Scalar[16];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    t[3 * i + j] = _matrix3[i]._Vector3[j];
                }
            }

        }

        public Vector3 getRowVector(int row)
        {
            return _matrix3[row];
        }

        public void setRowVector(int row, Vector3 v)
        {
            _matrix3[row] = new Vector3(v);
        }

        public Vector3 getColVector(int column)
        {
            Vector3 rv = new Vector3();
            rv._Vector3[0] = _matrix3[0]._Vector3[column];
            rv._Vector3[1] = _matrix3[1]._Vector3[column];
            rv._Vector3[2] = _matrix3[2]._Vector3[column];
            return rv;
        }

        public void setColVector(int column, Vector3 v)
        {
            _matrix3[0]._Vector3[column] = v._Vector3[0];
            _matrix3[1]._Vector3[column] = v._Vector3[1];
            _matrix3[2]._Vector3[column] = v._Vector3[2];
        }

        public Scalar[] GetStreamRowOrder()
        {
            Scalar[] result = new Scalar[9];
            int l = 0;
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    result[l++] = _matrix3[i]._Vector3[j];

            return result;
        }

        public Scalar[] GetStreamColumnOrder()
        {
            Scalar[] result = new Scalar[9];
            int l = 0;
            for (int j = 0; j < 3; j++)
                for (int i = 0; i < 3; i++)
                    result[l++] = _matrix3[i]._Vector3[j];
            return result;
        }

        public void Zero()
        {
            _matrix3[0].Zero();
            _matrix3[1].Zero();
            _matrix3[2].Zero();
        }

        public void Negate()
        {
            _matrix3[0].negate();
            _matrix3[1].negate();
            _matrix3[2].negate();
        }

        public void Print()
        {
            _matrix3[0].print();
            _matrix3[1].print();
            _matrix3[2].print();
        }

        public void Identity()
        {
            _matrix3[0].Zero();
            _matrix3[1].Zero();
            _matrix3[2].Zero();
            _matrix3[0]._Vector3[0] = 1.0F;
            _matrix3[1]._Vector3[1] = 1.0F;
            _matrix3[2]._Vector3[2] = 1.0F;
        }

        public Matrix3 transpose(Matrix3 m)
        {
            Matrix3 rm = new Matrix3();
            rm.setRowVector(0, m.getColVector(0));
            rm.setRowVector(1, m.getColVector(1));
            rm.setRowVector(2, m.getColVector(2));
            return rm;
        }

        //
        // inverse function 
        //
        public static Matrix3 inverse(Matrix3 m)
        {
            int i, j, k;
            double[,] work_mat = new double[3, 6];
            Matrix3 result = new Matrix3(new Vector3(), new Vector3(), new Vector3());

            //
            // setup a work matrix of Mx2M, the left part (MxM) is input matrix, 
            // the MxM right part as an identity matrix
            //  3x6 workmat:
            //                [ m0 m3 m6   1 0 0 ] 
            //                [ m1 m4 m7   0 1 0 ]
            //                [ m2 m5 m8   0 0 1 ]
            //
            for (i = 0; i < 3; i++)
            {
                for (j = 0; j < 3; j++)
                {
                    work_mat[i, j] = (double)m._Matrix3[i]._Vector3[j];
                    work_mat[i, j + 3] = 0.0;
                }
                work_mat[i, i + 3] = 1.0;
            }

            // find out the element with the largest absolute value for each row
            double[] row_max = new double[3];
            for (i = 0; i < 3; i++)
            {
                row_max[i] = System.Math.Abs(work_mat[i, 0]);
                for (j = 1; j < 3; j++)
                    if (System.Math.Abs(work_mat[i, j]) > row_max[i]) row_max[i] = System.Math.Abs(work_mat[i, j]);
                // singular matrix if containing all-0s row
                if (row_max[i] == 0.0) return result;
            }

            //
            // transform the left part into an upper triangle matrix using Gaussian
            // elimination.
            //     [ m0 m3 m6   1 0 0 ]         [ m0 m3 m6   r0 r3 r6 ]
            //     [ m1 m4 m7   0 1 0 ]   ==>   [ 0  m4 m7   r1 r4 r7 ]
            //     [ m2 m5 m8   0 0 1 ]         [ 0  0  m8   r2 r5 r8 ]
            //           
            for (i = 0; i < 3; i++)
            {
                // find target row for top
                int target = i;
                double col_row_max = System.Math.Abs(work_mat[i, i] / row_max[i]);
                for (j = i + 1; j < 3; j++)
                    if (System.Math.Abs(work_mat[j, i] / row_max[j]) > col_row_max)
                    {
                        target = j;
                        col_row_max = System.Math.Abs(work_mat[j, i] / row_max[j]);
                    }
                // swap the row with the largest col_row_max to the current work row
                if (target != i)
                {
                    // copy the ith row of work_mat to temp
                    double[] tmp = new double[6];
                    tmp[0] = work_mat[i, 0]; tmp[1] = work_mat[i, 1]; tmp[2] = work_mat[i, 2];
                    tmp[3] = work_mat[i, 3]; tmp[4] = work_mat[i, 4]; tmp[5] = work_mat[i, 5];
                    // copy the target row of work_map into ith row of work_map
                    work_mat[i, 0] = work_mat[target, 0]; work_mat[i, 1] = work_mat[target, 1]; work_mat[i, 2] = work_mat[target, 2];
                    work_mat[i, 3] = work_mat[target, 3]; work_mat[i, 4] = work_mat[target, 4]; work_mat[i, 5] = work_mat[target, 5];
                    // copy temp to the target row of work_map
                    work_mat[target, 0] = tmp[0]; work_mat[target, 1] = tmp[1]; work_mat[target, 2] = tmp[2];
                    work_mat[target, 3] = tmp[3]; work_mat[target, 4] = tmp[4]; work_mat[target, 5] = tmp[5];

                    // also, swap the row max of target with ith row one
                    double tmp2 = row_max[i];
                    row_max[i] = row_max[target];
                    row_max[target] = tmp2;
                }

                // execute gaussian elimination row by row
                for (j = i + 1; j < 3; j++)
                {
                    double factor = work_mat[j, i] / work_mat[i, i];
                    work_mat[j, i] = 0.0;
                    for (k = i + 1; k < 3 << 1; k++)
                        work_mat[j, k] -= work_mat[i, k] * factor;

                }
            }

            // singular if the rank is less than 3
            if (work_mat[2, 2] == 0.0F) return result;

            //
            // transfrom the left upper triangle into identity, the right part is
            // the inverse what we need
            //    [ m0 m3 m6   r0 r3 r6 ]         [ 1 0 0   r0 r3 r6 ]
            //    [ 0  m4 m7   r1 r4 r7 ]   ==>   [ 0 1 0   r1 r4 r7 ]
            //    [ 0  0  m8   r2 r5 r8 ]         [ 0 0 1   r2 r5 r8 ]
            //
            for (i = 2; i > 0; --i)
            {
                for (j = i - 1; j >= 0; --j)
                {
                    double factor = work_mat[j, i] / work_mat[i, i];
                    for (k = j + 1; k < 3 << 1; k++)
                        work_mat[j, k] -= factor * work_mat[i, k];

                }
            }

            // set the resulting inverse matrix
            for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                    result._Matrix3[i]._Vector3[j] = (Scalar)(work_mat[i, j + 3] / work_mat[i, i]);

            return result;
        }

        //
        // safe inverse, verfying the resulting inverse matrix
        //
        public static bool safe_inverse(Matrix3 m)
        {
            Matrix3 inv = new Matrix3();
            inv = Matrix3.inverse(m);

            Matrix3 l = inv * m;
            Matrix3 r = m * inv;

            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if (System.Math.Abs(l._Matrix3[i]._Vector3[j] - r._Matrix3[i]._Vector3[j]) > BoundCondition.SCALAR_EPSILON)
                        return false;
            return true;
        }

        // operator functions
        public static Matrix3 operator +(Matrix3 a, Matrix3 b)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0] = a._Matrix3[0] + b._Matrix3[0];
            temp._Matrix3[1] = a._Matrix3[1] + b._Matrix3[1];
            temp._Matrix3[2] = a._Matrix3[2] + b._Matrix3[2];
            return temp;
        }

        public static Matrix3 operator -(Matrix3 m)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0].negate();
            temp._Matrix3[1].negate();
            temp._Matrix3[2].negate();
            return temp;
        }

        public static Matrix3 operator -(Matrix3 a, Matrix3 b)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0] = a._Matrix3[0] - b._Matrix3[0];
            temp._Matrix3[1] = a._Matrix3[1] - b._Matrix3[1];
            temp._Matrix3[2] = a._Matrix3[2] - b._Matrix3[2];
            return temp;
        }

        public static Matrix3 operator *(Matrix3 a, Matrix3 b)
        {
            Matrix3 temp = new Matrix3();
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    temp._Matrix3[i]._Vector3[j] = a._Matrix3[i].dot(b.getColVector(j));
            return temp;
        }

        public static Matrix3 operator *(Scalar s, Matrix3 b)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0] = s * b._Matrix3[0];
            temp._Matrix3[1] = s * b._Matrix3[1];
            temp._Matrix3[2] = s * b._Matrix3[2];
            return temp;
        }

        public static Matrix3 operator *(Matrix3 a, Scalar s)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0] = a._Matrix3[0] * s;
            temp._Matrix3[1] = a._Matrix3[1] * s;
            temp._Matrix3[2] = a._Matrix3[2] * s;
            return temp;
        }

        public static Vector3 operator *(Matrix3 a, Vector3 v)
        {
            Vector3 temp = new Vector3();
            temp._Vector3[0] = a._Matrix3[0].dot(v);
            temp._Vector3[1] = a._Matrix3[1].dot(v);
            temp._Vector3[2] = a._Matrix3[2].dot(v);
            return temp;
        }

        public static Matrix3 operator /(Matrix3 a, Scalar s)
        {
            Matrix3 temp = new Matrix3();
            temp._Matrix3[0] = a._Matrix3[0] / s;
            temp._Matrix3[1] = a._Matrix3[1] / s;
            temp._Matrix3[2] = a._Matrix3[2] / s;
            return temp;
        }

        public static bool operator ==(Matrix3 a, Matrix3 b)
        {
            if (a._Matrix3[0] != b._Matrix3[0]) return false;
            if (a._Matrix3[1] != b._Matrix3[1]) return false;
            if (a._Matrix3[2] != b._Matrix3[2]) return false;
            return true;
        }

        public static bool operator !=(Matrix3 a, Matrix3 b)
        {
            return !(a == b);
        }

        public override bool Equals(Object obj)
        {
            return obj is Matrix3 && this == (Matrix3)obj;
        }

        public override int GetHashCode()
        {
            return _matrix3.GetHashCode();
        }
    }

    #endregion

    #region Matrix4

    //
    // Matrix4
    //
    [Serializable]
    public class Matrix4 : Matrix
    {
        // data member
        private Vector4[] _matrix4 = new Vector4[4];

        #region Constructors
        public Matrix4()
        {
            _matrix4[0] = new Vector4();
            _matrix4[1] = new Vector4();
            _matrix4[2] = new Vector4();
            _matrix4[3] = new Vector4();
            Identity();
        }

        public Matrix4(Vector4[] v)
        {
            _matrix4[0] = new Vector4(v[0]);
            _matrix4[1] = new Vector4(v[1]);
            _matrix4[2] = new Vector4(v[2]);
            _matrix4[3] = new Vector4(v[3]);
        }

        public Matrix4(Matrix4 m)
        {
            _matrix4[0] = new Vector4(m._Matrix4[0]);
            _matrix4[1] = new Vector4(m._Matrix4[1]);
            _matrix4[2] = new Vector4(m._Matrix4[2]);
            _matrix4[3] = new Vector4(m._Matrix4[3]);
        }

        public Matrix4(Vector4 a, Vector4 b, Vector4 c, Vector4 d)
        {
            _matrix4[0] = new Vector4(a);
            _matrix4[1] = new Vector4(b);
            _matrix4[2] = new Vector4(c);
            _matrix4[3] = new Vector4(d);
        }

        public Matrix4(Scalar m0, Scalar m1, Scalar m2, Scalar m3, 
                       Scalar m4, Scalar m5, Scalar m6, Scalar m7, 
                       Scalar m8, Scalar m9, Scalar m10, Scalar m11, 
                       Scalar m12, Scalar m13, Scalar m14, Scalar m15)
        {
            _matrix4[0] = new Vector4();
            _matrix4[1] = new Vector4();
            _matrix4[2] = new Vector4();
            _matrix4[3] = new Vector4();

            _matrix4[0]._Vector4[0] = m0;
            _matrix4[0]._Vector4[1] = m1;
            _matrix4[0]._Vector4[2] = m2;
            _matrix4[0]._Vector4[3] = m3;

            _matrix4[1]._Vector4[0] = m4;
            _matrix4[1]._Vector4[1] = m5;
            _matrix4[1]._Vector4[2] = m6;
            _matrix4[1]._Vector4[3] = m7;

            _matrix4[2]._Vector4[0] = m8;
            _matrix4[2]._Vector4[1] = m9;
            _matrix4[2]._Vector4[2] = m10;
            _matrix4[2]._Vector4[3] = m11;

            _matrix4[3]._Vector4[0] = m12;
            _matrix4[3]._Vector4[1] = m13;
            _matrix4[3]._Vector4[2] = m14;
            _matrix4[3]._Vector4[3] = m15;
        }

		public Matrix4(Scalar[] m)
		{
			_matrix4[0] = new Vector4();
			_matrix4[1] = new Vector4();
			_matrix4[2] = new Vector4();
			_matrix4[3] = new Vector4();

			_matrix4[0]._Vector4[0] = m[0];
			_matrix4[0]._Vector4[1] = m[4];
			_matrix4[0]._Vector4[2] = m[8];
			_matrix4[0]._Vector4[3] = m[12];

			_matrix4[1]._Vector4[0] = m[1];
			_matrix4[1]._Vector4[1] = m[5];
			_matrix4[1]._Vector4[2] = m[9];
			_matrix4[1]._Vector4[3] = m[13];

			_matrix4[2]._Vector4[0] = m[2];
			_matrix4[2]._Vector4[1] = m[6];
			_matrix4[2]._Vector4[2] = m[10];
			_matrix4[2]._Vector4[3] = m[14];

			_matrix4[3]._Vector4[0] = m[3];
			_matrix4[3]._Vector4[1] = m[7];
			_matrix4[3]._Vector4[2] = m[11];
			_matrix4[3]._Vector4[3] = m[15];
		}

		//
		// This constructor creates a rotation matrix from the angle and the axis of rotation.
		//
		public Matrix4(Scalar radians, Vector3 axis)
        {
            _matrix4[0] = new Vector4();
            _matrix4[1] = new Vector4();
            _matrix4[2] = new Vector4();
            _matrix4[3] = new Vector4();
            
            _matrix4[0].Zero();
            _matrix4[1].Zero();
            _matrix4[2].Zero();
            _matrix4[3].Zero();

            Scalar cosValue = (Scalar)System.Math.Cos(radians);
            Scalar sinValue = (Scalar)System.Math.Sin(radians);

			_matrix4[0]._Vector4[0] = cosValue + (1-cosValue) * axis.x*axis.x;
			_matrix4[0]._Vector4[1] = (1-cosValue)*axis.y*axis.x - sinValue*axis.z;
			_matrix4[0]._Vector4[2] = (1-cosValue)*axis.z*axis.x + sinValue*axis.y;

			_matrix4[1]._Vector4[0] = (1-cosValue)*axis.y*axis.x + sinValue*axis.z;
			_matrix4[1]._Vector4[1] = cosValue + (1-cosValue)*axis.y*axis.y;
			_matrix4[1]._Vector4[2] = (1-cosValue)*axis.z*axis.y - sinValue*axis.x;
			
			_matrix4[2]._Vector4[0] = (1-cosValue)*axis.z*axis.x - sinValue*axis.y;
			_matrix4[2]._Vector4[1] = (1-cosValue)*axis.z*axis.y + sinValue*axis.x;
			_matrix4[2]._Vector4[2] = cosValue + (1-cosValue)*axis.z*axis.z;
            _matrix4[3]._Vector4[3] = 1.0f;
        }
        #endregion

        // property
        public Vector4[] _Matrix4
        {
            get { return this._matrix4; }
            set { this._matrix4 = value; }
        }

        public Scalar this[int row, int column]
        {
            get
            {
                return _matrix4[row]._Vector4[column];
            }
            set
            {
                //
                // We could also do asserts, which would avoid performance penalties on 
                //    the release version. We really need this to be in-lined for the
                //    operators below.
                //
                if ((row >= 0) && (row < 4) && (column >= 0) && (column < 4))
                    _matrix4[row]._Vector4[column] = value;
                else
                    throw (new IndexOutOfRangeException());
            }
        }

        // public methods
        public void GetValue(out Scalar[] t)
        {
            t = new Scalar[16];
            if (t.Length == 16)
            {
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        t[j*4 + i] = _matrix4[i]._Vector4[j];
                    }
                }
            }
            else
            {
                Console.WriteLine("Should be the length of 16 for the array");
            }
        }

        public Vector4 getRowVector(int row)
        {
            return new Vector4(_matrix4[row]);
        }

        public void setRowVector(int row, Vector4 v)
        {
            _matrix4[row] = new Vector4(v);
        }

        public Vector4 getColVector(int column)
        {
            Vector4 rv = new Vector4();
            rv._Vector4[0] = _matrix4[0]._Vector4[column];
            rv._Vector4[1] = _matrix4[1]._Vector4[column];
            rv._Vector4[2] = _matrix4[2]._Vector4[column];
            rv._Vector4[3] = _matrix4[3]._Vector4[column];
            return rv;
        }

        public void setColVector(int column, Vector4 v)
        {
            _matrix4[0]._Vector4[column] = v._Vector4[0];
            _matrix4[1]._Vector4[column] = v._Vector4[1];
            _matrix4[2]._Vector4[column] = v._Vector4[2];
            _matrix4[3]._Vector4[column] = v._Vector4[3];
        }

        public Scalar[] GetStreamRowOrder()
        {
            Scalar[] result = new Scalar[16];
            int l = 0;
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    result[l++] = _matrix4[i]._Vector4[j];

            return result;
        }

        public Scalar[] GetStreamColumnOrder()
        {
            Scalar[] result = new Scalar[16];
            int l = 0;
            for (int j = 0; j < 4; j++)
                for (int i = 0; i < 4; i++)
                    result[l++] = _matrix4[i]._Vector4[j];
            return result;
        }

        public void Zero()
        {
            _matrix4[0].Zero();
            _matrix4[1].Zero();
            _matrix4[2].Zero();
            _matrix4[3].Zero();
        }

        public void Negate()
        {
            _matrix4[0].negate();
            _matrix4[1].negate();
            _matrix4[2].negate();
            _matrix4[3].negate();
        }

        public void Print()
        {
            _matrix4[0].print();
            _matrix4[1].print();
            _matrix4[2].print();
            _matrix4[3].print();
        }

        public void Identity()
        {
            _matrix4[0].Zero();
            _matrix4[1].Zero();
            _matrix4[2].Zero();
            _matrix4[3].Zero();
            _matrix4[0]._Vector4[0] = 1.0F;
            _matrix4[1]._Vector4[1] = 1.0F;
            _matrix4[2]._Vector4[2] = 1.0F;
            _matrix4[3]._Vector4[3] = 1.0F;
        }

        public Matrix4 transpose(Matrix4 m)
        {
            Matrix4 rm = new Matrix4();
            rm.setRowVector(0, m.getColVector(0));
            rm.setRowVector(1, m.getColVector(1));
            rm.setRowVector(2, m.getColVector(2));
            rm.setRowVector(3, m.getColVector(3));
            return rm;
        }

        //
        // inverse function 
        //
        public static Matrix4 inverse(Matrix4 m)
        {
            int i, j, k;
            double[,] work_mat = new double[4, 8];
            Matrix4 result = new Matrix4(new Vector4(), new Vector4(), new Vector4(), new Vector4());

            //
            // setup a work matrix of Mx2M, the left part (MxM) is input matrix, 
            // the MxM right part as an identity matrix
            //  4x8 workmat:
            //                [ m0 m4 m8  m12  1 0 0 ] 
            //                [ m1 m5 m9  m13  0 1 0 ]
            //                [ m2 m6 m10 m14  0 0 1 ]
            //                [ m3 m7 m11 m15  0 0 1 ]
            //
            for (i = 0; i < 4; i++)
            {
                for (j = 0; j < 4; j++)
                {
                    work_mat[i, j] = (double)m._Matrix4[i]._Vector4[j];
                    work_mat[i, j + 4] = 0.0;
                }
                work_mat[i, i + 4] = 1.0;
            }

            // find out the element with the largest absolute value for each row
            double[] row_max = new double[4];
            for (i = 0; i < 4; i++)
            {
                row_max[i] = System.Math.Abs(work_mat[i, 0]);
                for (j = 1; j < 4; j++)
                    if (System.Math.Abs(work_mat[i, j]) > row_max[i]) row_max[i] = System.Math.Abs(work_mat[i, j]);
                // singular matrix if containing all-0s row
                if (row_max[i] == 0.0) return result;
            }

            //
            // transform the left part into an upper triangle matrix using Gaussian
            // elimination.
            //     [ m0 m4 m8  m12  1 0 0 ]         [ m0 m4 m8  m12  r0 r4 r8  r12]
            //     [ m1 m5 m9  m13  0 1 0 ]   ==>   [ 0  m5 m9  m13  r1 r5 r9  r13]
            //     [ m2 m6 m10 m14  0 0 1 ]         [ 0  0  m10 m14  r2 r6 r10 r14]
            //     [ m3 m7 m11 m15  0 0 1 ]         [ 0  0  0   m15  r3 r7 r11 r15]
            //           
            for (i = 0; i < 4; i++)
            {
                // find target row for top
                int target = i;
                double col_row_max = System.Math.Abs(work_mat[i, i] / row_max[i]);
                for (j = i + 1; j < 4; j++)
                    if (System.Math.Abs(work_mat[j, i] / row_max[j]) > col_row_max)
                    {
                        target = j;
                        col_row_max = System.Math.Abs(work_mat[j, i] / row_max[j]);
                    }
                // swap the row with the largest col_row_max to the current work row
                if (target != i)
                {
                    // copy the ith row of work_mat to temp
                    double[] tmp = new double[8];
                    tmp[0] = work_mat[i, 0]; tmp[1] = work_mat[i, 1]; tmp[2] = work_mat[i, 2]; tmp[3] = work_mat[i, 3];
                    tmp[4] = work_mat[i, 4]; tmp[5] = work_mat[i, 5]; tmp[6] = work_mat[i, 6]; tmp[7] = work_mat[i, 7];
                    // copy the target row of work_map into ith row of work_map
                    work_mat[i, 0] = work_mat[target, 0]; work_mat[i, 1] = work_mat[target, 1];
                    work_mat[i, 2] = work_mat[target, 2]; work_mat[i, 3] = work_mat[target, 3]; 
                    work_mat[i, 4] = work_mat[target, 4]; work_mat[i, 5] = work_mat[target, 5];
                    work_mat[i, 6] = work_mat[target, 6]; work_mat[i, 7] = work_mat[target, 7];
                    // copy temp to the target row of work_map
                    work_mat[target, 0] = tmp[0]; work_mat[target, 1] = tmp[1];
                    work_mat[target, 2] = tmp[2]; work_mat[target, 3] = tmp[3]; 
                    work_mat[target, 4] = tmp[4]; work_mat[target, 5] = tmp[5];
                    work_mat[target, 6] = tmp[6]; work_mat[target, 7] = tmp[7];

                    // also, swap the row max of target with ith row one
                    double tmp2 = row_max[i];
                    row_max[i] = row_max[target];
                    row_max[target] = tmp2;
                }

                // execute gaussian elimination row by row
                for (j = i + 1; j < 4; j++)
                {
                    double factor = work_mat[j, i] / work_mat[i, i];
                    work_mat[j, i] = 0.0;
                    for (k = i + 1; k < 4 << 1; k++)
                        work_mat[j, k] -= work_mat[i, k] * factor;

                }
            }

            // singular if the rank is less than 3
            if (work_mat[3, 3] == 0.0F) return result;

            //
            // transfrom the left upper triangle into identity, the right part is
            // the inverse what we need
            //    [ m0 m4 m8  m12  r0 r4 r8  r12]         [ 1 0 0 0  r0 r4 r8  r12]
            //    [ 0  m5 m9  m13  r1 r5 r9  r13]   ==>   [ 0 1 0 0  r1 r5 r9  r13]
            //    [ 0  0  m10 m14  r2 r6 r10 r14]         [ 0 0 1 0  r2 r6 r10 r14]
            //    [ 0  0  m11 m15  r3 r7 r11 r15]         [ 0 0 0 1  r3 r7 r11 r15]
            //
            for (i = 3; i > 0; --i)
            {
                for (j = i - 1; j >= 0; --j)
                {
                    double factor = work_mat[j, i] / work_mat[i, i];
                    for (k = j + 1; k < 4 << 1; k++)
                        work_mat[j, k] -= factor * work_mat[i, k];

                }
            }

            // set the resulting inverse matrix
            for (i = 0; i < 4; i++)
                for (j = 0; j < 4; j++)
                    result._Matrix4[i]._Vector4[j] = (Scalar)(work_mat[i, j + 4] / work_mat[i, i]);

            return result;
        }

        //
        // safe inverse, verfying the resulting inverse matrix
        //
        public static bool safe_inverse(Matrix4 m)
        {
            Matrix4 inv = Matrix4.inverse(m);

            Matrix4 l = inv * m;
            Matrix4 r = m * inv;

            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    if (System.Math.Abs(l._Matrix4[i]._Vector4[j] - r._Matrix4[i]._Vector4[j]) > BoundCondition.SCALAR_EPSILON)
                        return false;
            //
            // Inverse exists. Copy it over;
            //
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    m[i, j] = inv[i, j];

            return true;
        }


        // operator functions
        public static Matrix4 operator +(Matrix4 a, Matrix4 b)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0] = a._Matrix4[0] + b._Matrix4[0];
            temp._Matrix4[1] = a._Matrix4[1] + b._Matrix4[1];
            temp._Matrix4[2] = a._Matrix4[2] + b._Matrix4[2];
            temp._Matrix4[3] = a._Matrix4[3] + b._Matrix4[3];
            return temp;
        }

        public static Matrix4 operator -(Matrix4 m)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0].negate();
            temp._Matrix4[1].negate();
            temp._Matrix4[2].negate();
            temp._Matrix4[3].negate();
            return temp;
        }

        public static Matrix4 operator -(Matrix4 a, Matrix4 b)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0] = a._Matrix4[0] - b._Matrix4[0];
            temp._Matrix4[1] = a._Matrix4[1] - b._Matrix4[1];
            temp._Matrix4[2] = a._Matrix4[2] - b._Matrix4[2];
            temp._Matrix4[3] = a._Matrix4[3] - b._Matrix4[3];
            return temp;
        }

        public static Matrix4 operator *(Matrix4 a, Matrix4 b)
        {
            Matrix4 temp = new Matrix4();
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    temp._Matrix4[i]._Vector4[j] = a._Matrix4[i].dot(b.getColVector(j));
            return temp;
        }
        

        public static Matrix4 operator *(Scalar s, Matrix4 b)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0] = s * b._Matrix4[0];
            temp._Matrix4[1] = s * b._Matrix4[1];
            temp._Matrix4[2] = s * b._Matrix4[2];
            temp._Matrix4[3] = s * b._Matrix4[3];
            return temp;
        }

        public static Matrix4 operator *(Matrix4 a, Scalar s)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0] = a._Matrix4[0] * s;
            temp._Matrix4[1] = a._Matrix4[1] * s;
            temp._Matrix4[2] = a._Matrix4[2] * s;
            temp._Matrix4[3] = a._Matrix4[3] * s;
            return temp;
        }

        public static Vector4 operator *(Matrix4 a, Vector4 v)
        {
            Vector4 rv = new Vector4();
            for (int i = 0; i < 4; i++)
                rv._Vector4[i] = a._Matrix4[i].dot(v);
            return rv;
        }

        public static Matrix4 operator /(Matrix4 a, Scalar s)
        {
            Matrix4 temp = new Matrix4();
            temp._Matrix4[0] = a._Matrix4[0] / s;
            temp._Matrix4[1] = a._Matrix4[1] / s;
            temp._Matrix4[2] = a._Matrix4[2] / s;
            temp._Matrix4[3] = a._Matrix4[3] / s;
            return temp;
        }

        public static bool operator ==(Matrix4 a, Matrix4 b)
        {
            if (a._Matrix4[0] != b._Matrix4[0]) return false;
            if (a._Matrix4[1] != b._Matrix4[1]) return false;
            if (a._Matrix4[2] != b._Matrix4[2]) return false;
            if (a._Matrix4[3] != b._Matrix4[3]) return false;
            return true;
        }

        public static bool operator !=(Matrix4 a, Matrix4 b)
        {
            return !(a == b);
        }

        public override bool Equals(Object obj)
        {
            return obj is Matrix4 && this == (Matrix4)obj;
        }

        public override int GetHashCode()
        {
            return _matrix4.GetHashCode();
        }
    }

    #endregion
}